package org.femto.math;
import org.femto.math.linalg.*;

public class Util {

    public static double clamp(double x, double xMin, double xMax) {
        if (x < xMin) {
            return xMin;
        } else if (x > xMax) {
            return xMax;
        } else {
            return x;
        }
    }

    public static int clamp(int x, int xMin, int xMax) {
        if (x < xMin) {
            return xMin;
        } else if (x > xMax) {
            return xMax;
        } else {
            return x;
        }
    }

    public Vec3 hsv2rgb(Vec3 hsv) {
        double hue = hsv.v[0];
        double sat = hsv.v[1];
        double val = hsv.v[2];

        double x = 0.0, y = 0.0, z = 0.0;

        if (hue == 1) {
            hue = 0;
        }
        else {
            hue *= 6;
        }

        int i = (int) Math.floor(hue);
        double f = hue-i;
        double p = val*(1-sat);
        double q = val*(1-(sat*f));
        double t = val*(1-(sat*(1-f)));

        switch (i) {
          case 0: x = val; y = t; z = p; break;
          case 1: x = q; y = val; z = p; break;
          case 2: x = p; y = val; z = t; break;
          case 3: x = p; y = q; z = val; break;
          case 4: x = t; y = p; z = val; break;
          case 5: x = val; y = p; z = q; break;
        }

        return new Vec3(x,y,z);
    }

    /**
     * @param p p = [x,y,z]
     * @return [r, phi, theta]
     */
    public static Vec3 cartesian2spherical(Vec3 p) {
        return new Vec3(p.norm(), Math.acos(p.v[2]/p.norm()), Math.atan2(p.v[1], p.v[0]));
    }

    /** rescale and translate a point x in the interval [a,b] to a point in the interval [c,d] */
    public static double remap(double x, double a, double b, double c, double d) {
        return x*(c-d)/(a-b) + (a*d-b*c)/(a-b);
    }

    public static double unitangle(Vec3 u, Vec3 v) {
        if(Vec3.dot(u,v) < 0.0) {
            return Math.PI - 2*Math.asin(v.add(u).negative().norm()/2);
        }
        else {
            return 2*Math.asin(Vec3.dist(v, u)/2);
        }
    }

    public static double sinxoverx(double x) {
        if (1.0 + x*x == 1.0) {
            return 1.0;
        } else {
            return Math.sin(x) / x;
        }
    }

    public static Vec3 slerp(Vec3 v0, Vec3 v1, double t) {
        double theta = unitangle(v0, v1);
        double c0 = (1-t)*sinxoverx((1-t)*theta)/sinxoverx(theta);
        double c1 = t*sinxoverx(t*theta)/sinxoverx(theta);
        return Vec3.add(v0.scale(c0), v1.scale(c1));
    }

    public static float lerp(float f0, float f1, float t) {
        return (1.0f-t)*f0 + t*f1;
    }

    public static float bilerp(float f00, float f10, float f01, float f11, float tx, float ty) {
        return lerp(lerp(f00, f10, tx), lerp(f01, f11, tx), ty);
    }

    public static float trilerp(float f000, float f100, float f010, float f110, float f001, float f101,
            float f011, float f111, float tx, float ty, float tz) {
        return lerp(bilerp(f000, f100, f010, f110, tx, ty), bilerp(f001, f101, f011, f111, tx, ty), tz);
    }
    
}
